package com.jsurf;

import com.sun.jna.Memory;
import com.sun.jna.Pointer;

import java.util.LinkedList;
import java.util.List;

import static name.audet.samuel.javacv.jna.cxcore.*;

public class FastHessian {
	
	//! Filter index map
	private static int[][] filter_map = {{0, 1, 2, 3}, {1, 3, 4, 5}, {3, 5, 6, 7}, {5, 7, 8, 9}, {7, 9, 10, 11}};
	
	private List<InterestPoint> mInterestPoints;
	
	//! Number of Octaves
	private int mOctaves;
	
	//! Initial sampling step for Ipoint detection
	private int mInitSample;
	
	//! Threshold value for blob resonses
	private float mThreshold;
	
	//! Integral Image, and its attributes
	private IntegralImage mIntegralImage;
	private int mHeight;
	private int mWidth;
	
	private boolean mRecalculateInterestPoints = true;
	
	//! Response list of determinant of hessian values
	List<ResponseLayer> mLayers;
	
	public FastHessian(IntegralImage integralImage,	int octaves, int initSample, 
							float threshold) {
		mIntegralImage = integralImage;
		mOctaves = octaves;
		mInitSample = initSample;
		mThreshold = threshold;
		mWidth = integralImage.getWidth();
		mHeight = integralImage.getHeight();
	}
	
	//! Find the image features and write into list of features
	public List<InterestPoint> getIPoints() {
		//TODO: Delete this condition
		if (mInterestPoints == null || mRecalculateInterestPoints) {
			mInterestPoints = new LinkedList<InterestPoint>();
			
			// Build the response map
			buildResponseMap();
			
			// Get the response layers
			ResponseLayer b, m, t;
			for (int o = 0; o < mOctaves; ++o) {
				for (int i = 0; i <= 1; ++i) {
				    b = mLayers.get(filter_map[o][i]);
				    m = mLayers.get(filter_map[o][i + 1]);
				    t = mLayers.get(filter_map[o][i + 2]);
				    
				    // loop over middle response layer at density of the most
				    // sparse layer (always top), to find maxima across scale and spaceID
				    for (int r = 0; r < t.getHeight(); ++r){
				    	for (int c = 0; c < t.getWidth(); ++c){
				    		if (isExtremum(r, c, t, m, b)){
				    			InterestPoint point = interpolateExtremum(r, c, t, m, b);
				    			if (point != null){
				    				mInterestPoints.add(point);
				    			}
				    		}
				    	}
				    }
				}
			}
		}
		return mInterestPoints;
	}
	
	//! Build map of DoH responses
	private void buildResponseMap() {
		// Calculate responses for the first 4 octaves:
		// Oct1: 9,  15, 21, 27
		// Oct2: 15, 27, 39, 51
		// Oct3: 27, 51, 75, 99
		// Oct4: 51, 99, 147, 195
		// Oct5: 99, 195, 291, 387
		
		// Clear any existing response layers
		mLayers = new LinkedList<ResponseLayer>();
		
		// Get image attributes
		int w = mWidth / mInitSample;
		int h = mHeight / mInitSample;
		int s = mInitSample;
		
		// Calculate approximated determinant of hessian values
		if (mOctaves >= 1){
			mLayers.add(new ResponseLayer(w, h, s, 9, mIntegralImage));
			mLayers.add(new ResponseLayer(w, h, s, 15, mIntegralImage));
			mLayers.add(new ResponseLayer(w, h, s, 21, mIntegralImage));
			mLayers.add(new ResponseLayer(w, h, s, 27, mIntegralImage));
		}
		
		if (mOctaves >= 2){
			mLayers.add(new ResponseLayer(w / 2, h / 2, s * 2, 39, mIntegralImage));
			mLayers.add(new ResponseLayer(w / 2, h / 2, s * 2, 51, mIntegralImage));
		}

		if (mOctaves >= 3){
			mLayers.add(new ResponseLayer(w / 4, h / 4, s * 4, 75, mIntegralImage));
			mLayers.add(new ResponseLayer(w / 4, h / 4, s * 4, 99, mIntegralImage));

		}
		
		if (mOctaves >= 4){
			mLayers.add(new ResponseLayer(w / 8, h / 8, s * 8, 147, mIntegralImage));
			mLayers.add(new ResponseLayer(w / 8, h / 8, s * 8, 195, mIntegralImage));
		}
		
		if (mOctaves >= 5){
			mLayers.add(new ResponseLayer(w / 16, h / 16, s * 16, 291, mIntegralImage));
			mLayers.add(new ResponseLayer(w / 16, h / 16, s * 16, 387, mIntegralImage));
		}
	}
	
	//! Non Maximal Suppression function
	private boolean isExtremum(int r, int c, ResponseLayer t, ResponseLayer m, ResponseLayer b) {
		// Bounds check
		int layerBorder = (t.getFilter() + 1) / (2 * t.getStep());
		if (r <= layerBorder || r >= t.getHeight() - layerBorder || c <= layerBorder || c >= t.getWidth() - layerBorder)
			return false;
			
		// Check if the candidate point in the middle layer is above thresh 
		double candidate = m.getResponse(r, c, t);
		if (candidate < mThreshold)
			return false;
		
		// See if the response in 3x3x3 is greater, then it isn't a local maxima
		for (int rr = -1; rr <= 1; ++rr) {
			for (int cc = -1; cc <= 1; ++cc) {
				if (t.getResponse(r + rr, c + cc) >= candidate ||
					((rr != 0 || cc != 0) && m.getResponse(r + rr, c + cc, t) >= candidate) ||
					b.getResponse(r + rr, c + cc, t) >= candidate) {
					return false;
				}
			}
		}
		return true;
	}
	
	//! Interpolate scale-spaceID extrema to subpixel accuracy to form an image feature.
	private InterestPoint interpolateExtremum(int r, int c, ResponseLayer t, ResponseLayer m, ResponseLayer b){
		// Get the step distance between filters
  		// Check the middle filter (m) is mid way between top (t) and bottom (b)
		int filterStep = m.getFilter() - b.getFilter();
		if (!(filterStep > 0 && t.getFilter() - m.getFilter() == m.getFilter() - b.getFilter())) {
			System.out.println("FastHessian::interpolateExtremum : Debe abortarse la ejecución (OpenSURF c++).");
		}
		
		// Get the offsets to the actual location of the extremum
		double xi = 0, xr = 0, xc = 0;
		double[] values = interpolateStep(r, c, t, m, b);
		xi = values[0];
		xr = values[1];
		xc = values[2];
		
		if ((float)Math.abs(xi) < 0.5f & (float)Math.abs(xr) < 0.5f && (float)Math.abs(xc) < 0.5f){
			// INTEREST POINT FOUND
			float x = (float)((c + xc) * t.getStep());
			float y = (float)((r + xr) * t.getStep());
			float scale = (float)(0.1333f * (m.getFilter() + xi * filterStep));
			int laplacian = (int) m.getLaplacian(r, c, t);
			return new InterestPoint(x, y, scale, laplacian);
		}
		return null;
	}
	
	//! Performs one step of extremum interpolation. 
	private double[] interpolateStep(int r, int c, ResponseLayer t, ResponseLayer m, ResponseLayer b){
		CvMat dM, H, H_inv, X = new CvMat();
		
		double values[] = {0, 0, 0};
		
		// Create pointer to values -> function cvInitMatHeader receives a pointer
		Pointer p = new Memory(Float.SIZE / 8 * values.length * 2); // * 2 ?
		p.write(0, values, 0, values.length);

		dM = getPartialDerivativeMatrix(r, c, t, m, b);
		H = getHessian3DMatrix(r, c, t, m, b);
		H_inv = cvCreateMat(3, 3, CV_64FC1);
		cvInvert(H, H_inv, CV_SVD);
		cvInitMatHeader(X, 3, 1, CV_64FC1, p, CV_AUTO_STEP);
		cvGEMM(H_inv, dM, -1, null, 0, X, 0);
		
		dM.release();
		H.release();
		H_inv.release();

		values = p.getDoubleArray(0, values.length);
		return values;
	}
	
	//! Computes the partial derivatives in x, y, and scale of a pixel.
	private CvMat getPartialDerivativeMatrix(int r, int c, ResponseLayer t, ResponseLayer m, ResponseLayer b) {
		CvMat dM;
		double dx, dy, ds;
		
		dx = (m.getResponse(r, c + 1, t) - m.getResponse(r, c - 1, t)) / 2.0;
		dy = (m.getResponse(r + 1, c, t) - m.getResponse(r - 1, c, t)) / 2.0;
		ds = (t.getResponse(r, c) - b.getResponse(r, c, t)) / 2.0;
		
		dM = cvCreateMat(3, 1, CV_64FC1);
		dM.put(0, 0, dx);
		dM.put(1, 0, dy);
		dM.put(2, 0, ds);
		
		return dM;
	}
	
	//! Computes the 3D Hessian matrix for a pixel.
	private CvMat getHessian3DMatrix(int r, int c, ResponseLayer t, ResponseLayer m, ResponseLayer b) {
		// Layout:
		// [dxx][dxy][dxs]
		// [dxy][dyy][dys]
		// [dxs][dys][dss]
		
		CvMat H;
		double v, dxx, dyy, dss, dxy, dxs, dys;
		
		v = m.getResponse(r, c, t);

		dxx = m.getResponse(r, c + 1, t) + m.getResponse(r, c - 1, t) - 2 * v;
		dyy = m.getResponse(r + 1, c, t) + m.getResponse(r - 1, c, t) - 2 * v;
		dss = t.getResponse(r, c) + b.getResponse(r, c, t) - 2 * v;
		dxy = (m.getResponse(r + 1, c + 1, t) - m.getResponse(r + 1, c - 1, t) - 
			  m.getResponse(r - 1, c + 1, t) + m.getResponse(r - 1, c - 1, t)) / 4.0;
		dxs = (t.getResponse(r, c + 1) - t.getResponse(r, c - 1) - 
			  b.getResponse(r, c + 1, t) + b.getResponse(r, c - 1, t)) / 4.0;
		dys = (t.getResponse(r + 1, c) - t.getResponse(r - 1, c) - 
			  b.getResponse(r + 1, c, t) + b.getResponse(r - 1, c, t) ) / 4.0;
		
		H = cvCreateMat(3, 3, CV_64FC1);
		H.put(0, 0, dxx);
		H.put(0, 1, dxy);
		H.put(0, 2, dxs);
		H.put(1, 0, dxy);
		H.put(1, 1, dyy);
		H.put(1, 2, dys);
		H.put(2, 0, dxs);
		H.put(2, 1, dys);
		H.put(2, 2, dss);
		
		return H;
	}
}